#include <follow.h>
const double MY_PI = 3.1415;
const double MAX_DISTANCE = 5.;
const double MIN_DISTANCE = 0.5;
const double MAX_SPEED = 0.5;
const double DEFAULT_DISTANCE = 1.5;
double Setpoint = DEFAULT_DISTANCE, Input, Output;
double Kp=3., Ki=10, Kd=2;    //2 5 1
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVERSE); //DIRECT
double is_missing = true;
double global_goal[3] = {DEFAULT_DISTANCE,0.0,0.0};
void Follow::onInit()
{
    //map
    double r = nh_.param<double>("resolution",0.1);
    unsigned int w = nh_.param<double>("width",10.0) / r;
    unsigned int h = nh_.param<double>("height",10.0) / r;
    init_map(h,w,r);
    follow_map_ = get_char_map();
    //apf
    double AK = nh_.param<double>("Attraction_K",10);
    double RK = nh_.param<double>("Repulsion_K",32);
    double aa = nh_.param<double>("a",1.2);
    double Obs_dis = nh_.param<double>("Obstacles_dis",0.6);
    init_apf(AK,RK,aa,Obs_dis);
    double  pos[3] = {DEFAULT_DISTANCE,0.0,0.0};
    set_goal(pos);

    //control
    control_period_ms = nh_.param<int>("control_period_ms",100);
    myPID.SetMode(AUTOMATIC);				//turn the PID on
    myPID.SetSampleTime(control_period_ms);		//set pis control period
    myPID.SetOutputLimits(-10, 10);
    angular_gain = nh_.param<double>("angular_gain",1.0);

    //laser_sub_ = nh_.subscribe("/laser_scan",2,&Follow::laserCallback,this);
    goal_sub_ = nh_.subscribe("/tracker_node/tracked_object",2,&Follow::setGoalCallback,this);
    cmd_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel",2);
    thread t1(&Follow::control_loop,this); 
    t1.detach();
}
void Follow::setGoalCallback(const common_msg::TrackedObjectConstPtr& msg)
{
    is_missing = missing = msg->is_missing;
    global_goal[0] = msg->position.x/1000.;
    global_goal[1] = msg->position.y/1000.;
    return;

    double pos[3];
    missing = msg->is_missing;
    if(missing) return ;
    pos[0] = msg->position.x/1000.;
    pos[1] = msg->position.y/1000.;
    set_goal(pos);
}
void Follow::laserCallback(const sensor_msgs::LaserScanConstPtr& msg)
{
    float epsilon = 0.0001;  // a tenth of a millimeter
    sensor_msgs::LaserScan message = *msg;
    for (size_t i = 0; i < message.ranges.size(); i++)
    {
        float range = message.ranges[ i ];
        if (!std::isfinite(range) && range > 0)
        {
            message.ranges[ i ] = message.range_max - epsilon;
        }
    }    // project the laser into a point cloud
    sensor_msgs::PointCloud2 cloud;
    cloud.header = message.header;
    laser_geometry::LaserProjection projector_;
    // project the scan into a point cloud
    try
    {
        projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, tf_);
    }
    catch (tf::TransformException &ex)
    {
        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame");
        projector_.projectLaser(message, cloud);
    }

    pcl::PointCloud <pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl_cloud->header.frame_id = cloud.header.frame_id;
    try
    {
        pcl::PCLPointCloud2 pcl_pc2;
        pcl_conversions::toPCL(cloud, pcl_pc2);
        // Actually convert the PointCloud2 message into a type we can reason about
        pcl::fromPCLPointCloud2(pcl_pc2, *pcl_cloud);
    }
    catch (pcl::PCLException& ex)
    {
        ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what());
        return ;
    }
    /*************VoxelGrild Filter*************/
    pcl::PointCloud<pcl::PointXYZ> filteredCloud;
    pcl::VoxelGrid<pcl::PointXYZ> filter;
    filter.setInputCloud(pcl_cloud);
    filter.setLeafSize(0.05f, 0.05f, 0.05f);
    filter.filter(filteredCloud);

    /******************apf plan*****************/
    get_obstacle(filteredCloud);
    missing = is_missing;
    if(!missing)
    {
        double angle = atan2(global_goal[1],global_goal[0]);
        goal_point[0] = global_goal[0];// - cos(angle)*DEFAULT_DISTANCE;
        goal_point[1] = global_goal[1];// - sin(angle)*DEFAULT_DISTANCE;
        dis_to_goal = sqrt(pow(goal_point[0]-start_point[0],2)+\
        pow(goal_point[1]-start_point[1],2));
    }
    apf_angle = get_apf_angle();
}

void Follow::update_map(pcl::PointCloud <pcl::PointXYZ> cloud)
{

}
void Follow::get_obstacle(pcl::PointCloud<pcl::PointXYZ> cloud)
{
    double w = get_width()*get_resolution();
    double h = get_height()*get_resolution();
    vector<vector<double> > obs;
    for(int i=0;i<cloud.size();i++)
    {
        if(fabs(cloud.points[i].x)<w && fabs(cloud.points[i].y<h))
        {
            vector <double> temp;
            temp.push_back(cloud.points[i].x);
            temp.push_back(cloud.points[i].y);
            obs.push_back(temp);
        }
    }
    obstacles = obs;
    obs_num = obstacles.size();
}

void Follow::control_loop()
{
    sleep(2);
    geometry_msgs::Twist vel;
    ros::Rate r(1000/control_period_ms);
    while(ros::ok())
    {        
        missing = is_missing;
        vel.linear.x = 0.0;
        vel.angular.z = 0.0;
        if(!missing)
        {
            double angle = atan2(global_goal[1],global_goal[0]);
            goal_point[0] = global_goal[0];// - cos(angle)*DEFAULT_DISTANCE;
            goal_point[1] = global_goal[1];// - sin(angle)*DEFAULT_DISTANCE;
            dis_to_goal = sqrt(pow(goal_point[0]-start_point[0],2)+\
                pow(goal_point[1]-start_point[1],2));
            apf_angle = get_apf_angle();

            if(dis_to_goal>DEFAULT_DISTANCE - 0.15 && dis_to_goal<DEFAULT_DISTANCE + 0.15)
            {
                Input = dis_to_goal;
                myPID.Compute();
                vel.linear.x = 0.0;
                if(abs(apf_angle)<0.2)
                    vel.angular.z = 0.0;
                else 
                    vel.angular.z = apf_angle*angular_gain;
            }

            else if(dis_to_goal < MIN_DISTANCE || dis_to_goal > MAX_DISTANCE)
            {
                Input = dis_to_goal < MIN_DISTANCE ? MIN_DISTANCE : MAX_DISTANCE;
                myPID.Compute();
                vel.linear.x = 0.0;
                vel.angular.z = 0.0;
            }
            else
            {
                Input = dis_to_goal;
                myPID.Compute();
                float speed = Output * MAX_SPEED / 10;
                vel.linear.x = speed;
                ROS_INFO("vel_x:%f",speed);
                vel.angular.z = apf_angle*angular_gain;
            }
            cmd_pub_.publish(vel);
        }
        else
            ROS_INFO("track object missing");
        r.sleep();   
    }    
}
